import RPi.GPIO as GPIO
import time


class Servo:
    angle = 90
    is_restricted = False
    def __init__(self, servo_pin, is_restricted=False):
        if servo_pin not in [12, 13, 18, 19, 40]:
            raise Exception('servo_pin must in [12, 13, 18, 19, 40]')
        GPIO.setmode(GPIO.BCM)  # BCM引脚编号模式
        GPIO.setwarnings(False)
        GPIO.setup(servo_pin, GPIO.OUT)
        self.pwm = GPIO.PWM(servo_pin, 50)  # 初始化位置90°
        self.pwm.start(50)
        self.__set_pwm__(7.5)
        self.is_restricted = is_restricted

    def __del__(self):
        self.pwm.stop()
        GPIO.cleanup()

    def __set_pwm__(self, pwm):  # 2.5~12.5
        if pwm < 2.5 or pwm > 12.5:
            raise Exception('PWM is out of range (2.5,12.5)')
        self.pwm.ChangeDutyCycle(pwm)
    
    def __set_cur_pwm__(self):
        self.__set_pwm__(self.angle / 180 * 10 + 2.5)
        
    def rotate(self, angle_per_cyle=2, time_slot=0.3): # 3'/100ms
        # 45~135(is_restricted=True), 0~180(is_restricted=False)
        if self.is_restricted and 45 <= self.angle + angle_per_cyle <= 135:
            self.angle = self.angle + angle_per_cyle
        elif not self.is_restricted and 0 <= self.angle + angle_per_cyle <= 180:
            self.angle = self.angle + angle_per_cyle
        self.__set_cur_pwm__()
        time.sleep(time_slot)



if __name__ == '__main__':
    servo1 = Servo(19)
    servo2 = Servo(18, is_restricted=True)
    for i in range(10):
        servo1.rotate()
        servo2.rotate()
    time.sleep(1)
